import CONSTANT from "../constant";
import MassData from "./mass-data";
import Transform from "../common/transform";
import Vec3 from "../common/vec3";
import Quat from "../common/quat";
import Method from "../common/method";
import Mat3 from "../common/mat3";

export default class RigidBody {
    constructor(config) {
        this._name=config.name;
        this._next = null;
        this._prev = null;
        this._shapeList = null;
        this._shapeListLast = null;
        this._numShapes = 0;
        this._contactLinkList = null;
        this._contactLinkListLast = null;
        this._numContactLinks = 0;
        this._jointLinkList = null;
        this._numJointLinks = 0;
        let v = config.linearVelocity.elements;
        this.vel=new Float64Array(3);
        Method.copyElements(v,this.vel);
        let v1 = config.angularVelocity.elements;
        this.angVel=new Float64Array(3);
        Method.copyElements(v1,this.angVel);
        this.pseudoVel=new Float64Array(3);
        this.angPseudoVel = new Float64Array(3);
        this._ptransform = new Transform();
        Method.combineMat3Vec3ToTransform(config.position,config.rotation,this._ptransform);
        this._transform = new Transform();
        Method.copyTransform(this._ptransform,this._transform);
        this._type = config.type;
        this._sleepTime = 0;
        this._sleeping = false;
        this._autoSleep = config.autoSleep;
        this._mass = 0;
        this._invMass = 0;
        this.localInertia = new Float64Array(9);
        this.invLocalInertia = new Float64Array(9);
        this.invLocalInertiaWithoutRotFactor = new Float64Array(9);
        this.invInertia = new Float64Array(9);
        this._linearDamping = config.linearDamping;
        this._angularDamping = config.angularDamping;
        this.force = new Float64Array(3);
        this.torque = new Float64Array(3);
        this.linearContactImpulse = new Float64Array(3);
        this.angularContactImpulse = new Float64Array(3);
        this._rotFactor = new Vec3(1,1,1);
        this._addedToIsland = false;
        this._gravityScale = 1;
        this._world = null;
        this._object3Ds=[];
        this._tV0=new Vec3();
        this._tV1=new Vec3();
        this._tV2=new Vec3();
        this._tQ0=new Quat();
        this._tM0=new Mat3();
    }

    //.................................................public...................................................

    get name(){
        return this._name;
    }
    get object3Ds(){
        return this._object3Ds;
    }
    get isSleeping() {
        return this._sleeping;
    }
    get sleepTime() {
        return this._sleepTime;
    }
    set autoSleep(autoSleepEnabled) {
        this._autoSleep = autoSleepEnabled;
        this._sleeping = false;
        this._sleepTime = 0;
    }
    get linearDamping() {
        return this._linearDamping;
    }
    set linearDamping(damping) {
        this._linearDamping = damping;
    }
    get angularDamping() {
        return this._angularDamping;
    }
    set angularDamping(damping) {
        this._angularDamping = damping;
    }
    get prev() {
        return this._prev;
    }
    get next() {
        return this._next;
    }
    get numShapes() {
        return this._numShapes;
    }
    get shapeList() {
        return this._shapeList;
    }
    get numContactLinks() {
        return this._numContactLinks;
    }
    get contactLinkList() {
        return this._contactLinkList;
    }
    get numJointLinks() {
        return this._numJointLinks;
    }
    get jointLinkList() {
        return this._jointLinkList;
    }
    get mass() {
        return this._mass;
    }
    get gravityScale() {
        return this._gravityScale;
    }
    set gravityScale(gravityScale) {
        this._gravityScale = gravityScale;
        this._sleeping = false;
        this._sleepTime = 0;
    }
    getType() {
        return this._type;
    }
    setType(type) {
        this._type = type;
        this.updateMass();
    }
    addShape(shape) {
        if(this._shapeList == null) {
            this._shapeList = shape;
            this._shapeListLast = shape;
        } else {
            this._shapeListLast._next = shape;
            shape._prev = this._shapeListLast;
            this._shapeListLast = shape;
        }
        this._numShapes++;
        shape._rigidBody = this;
        if(this._world != null) {
            let _this = this._world;
            shape._proxy = _this._broadPhase.createProxy(shape,shape._aabb);
            shape._id = _this._shapeIdCount++;
            _this._numShapes++;
        }
        this.updateMass();
        this._updateShapeList();
    }
    removeShape(shape) {
        let prev = shape._prev;
        let next = shape._next;
        if(prev != null) {
            prev._next = next;
        }
        if(next != null) {
            next._prev = prev;
        }
        if(shape === this._shapeList) {
            this._shapeList = this._shapeList._next;
        }
        if(shape === this._shapeListLast) {
            this._shapeListLast = this._shapeListLast._prev;
        }
        shape._next = null;
        shape._prev = null;
        this._numShapes--;
        shape._rigidBody = null;
        if(this._world != null) {
            let _this = this._world;
            _this._broadPhase.destroyProxy(shape._proxy);
            shape._proxy = null;
            shape._id = -1;
            let cl = shape._rigidBody._contactLinkList;
            while(cl != null) {
                let n = cl._next;
                let c = cl._contact;
                if(c._s1 === shape || c._s2 === shape) {
                    let _this1 = cl._other;
                    _this1._sleeping = false;
                    _this1._sleepTime = 0;
                    let _this2 = _this._contactManager;
                    let prev = c._prev;
                    let next = c._next;
                    if(prev != null) {
                        prev._next = next;
                    }
                    if(next != null) {
                        next._prev = prev;
                    }
                    if(c === _this2._contactList) {
                        _this2._contactList = _this2._contactList._next;
                    }
                    if(c === _this2._contactListLast) {
                        _this2._contactListLast = _this2._contactListLast._prev;
                    }
                    c._next = null;
                    c._prev = null;
                    if(c._touching) {
                        let cc1 = c._s1._contactCallback;
                        let cc2 = c._s2._contactCallback;
                        if(cc1 === cc2) {
                            cc2 = null;
                        }
                        if(cc1 != null) {
                            cc1.endContact(c);
                        }
                        if(cc2 != null) {
                            cc2.endContact(c);
                        }
                    }
                    let prev1 = c._link1._prev;
                    let next1 = c._link1._next;
                    if(prev1 != null) {
                        prev1._next = next1;
                    }
                    if(next1 != null) {
                        next1._prev = prev1;
                    }
                    if(c._link1 === c._b1._contactLinkList) {
                        c._b1._contactLinkList = c._b1._contactLinkList._next;
                    }
                    if(c._link1 === c._b1._contactLinkListLast) {
                        c._b1._contactLinkListLast = c._b1._contactLinkListLast._prev;
                    }
                    c._link1._next = null;
                    c._link1._prev = null;
                    let prev2 = c._link2._prev;
                    let next2 = c._link2._next;
                    if(prev2 != null) {
                        prev2._next = next2;
                    }
                    if(next2 != null) {
                        next2._prev = prev2;
                    }
                    if(c._link2 === c._b2._contactLinkList) {
                        c._b2._contactLinkList = c._b2._contactLinkList._next;
                    }
                    if(c._link2 === c._b2._contactLinkListLast) {
                        c._b2._contactLinkListLast = c._b2._contactLinkListLast._prev;
                    }
                    c._link2._next = null;
                    c._link2._prev = null;
                    c._b1._numContactLinks--;
                    c._b2._numContactLinks--;
                    c._link1._other = null;
                    c._link2._other = null;
                    c._link1._contact = null;
                    c._link2._contact = null;
                    c._s1 = null;
                    c._s2 = null;
                    c._b1 = null;
                    c._b2 = null;
                    c._touching = false;
                    c._cachedDetectorData._clear();
                    c._manifold._clear();
                    c._detector = null;
                    let _this3 = c._contactConstraint;
                    _this3._s1 = null;
                    _this3._s2 = null;
                    _this3._b1 = null;
                    _this3._b2 = null;
                    _this3._tf1 = null;
                    _this3._tf2 = null;
                    c._next = _this2._contactPool;
                    _this2._contactPool = c;
                    _this2._numContacts--;
                }
                cl = n;
            }
            _this._numShapes--;
        }
        this.updateMass();
        this._updateShapeList();
    }
    addObject3D(...object3Ds){
        for(let obj of object3Ds){
            let idx=this._object3Ds.findIndex((o)=>o.uuid===obj.uuid);
            if(idx<0){
                this._object3Ds.push(obj);
                obj.userData.rigidBody=this;
            }
        }
    }
    removeObject3D(...object3Ds){
        for(let obj of object3Ds){
            let idx=this._object3Ds.findIndex((o)=>o.uuid===obj.uuid);
            if(idx<0)continue;
            this._object3Ds.splice(idx,1);
            obj.userData.rigidBody=null;
        }
    }
    clearObject3D(){
        this.removeObject3D(...this._object3Ds);
    }
    updateObject3Ds(){
        for(let o3d of this._object3Ds){
            this.getPositionTo(o3d.position);
            this.getOrientationTo(o3d.quaternion);
        }
    }
    translate(_translation) {
        let translation=Method.convertVec3(_translation,this._tV0).elements;
        let diffX = translation[0],diffY = translation[1],diffZ = translation[2];
        let transform=this._transform.elements;
        transform[0] += diffX;transform[1] += diffY;transform[2] += diffZ;
        Method.copyTransform(this._transform,this._ptransform);
        this._updateShapeList();
        this._sleeping = false;
        this._sleepTime = 0;
    }
    getPosition() {
        Method.extractVec3FromTransform(this._transform,this._tV0);
        return Method.copyVec3(this._tV0);
    }
    getPositionTo(position) {
        let transform=this._transform.elements;
        Method.setXYZ(position,transform[0],transform[1],transform[2]);
    }
    setPosition(_position) {
        let position=Method.convertVec3(_position,this._tV0).elements;
        let transform=this._transform.elements;
        transform[0] = position[0];transform[1] = position[1];transform[2] = position[2];
        Method.copyTransform(this._transform,this._ptransform);
        this._updateShapeList();
        this._sleeping = false;
        this._sleepTime = 0;
    }
    updateMass() {
        let totalInertia00 = 0,totalInertia01 = 0,totalInertia02 = 0;
        let totalInertia10 = 0,totalInertia11 = 0,totalInertia12 = 0;
        let totalInertia20 = 0,totalInertia21 = 0,totalInertia22 = 0;
        let totalMass = 0;
        let s = this._shapeList;
        while(s != null) {
            let n = s._next;
            let g = s._geom;
            g._updateMass();
            let mass = s._density * g._volume;
            let sTransform=s._localTransform.elements,gi=g.inertiaCoeff;
            let inertia00 = sTransform[3] * gi[0] + sTransform[4] * gi[3] + sTransform[5] * gi[6];
            let inertia01 = sTransform[3] * gi[1] + sTransform[4] * gi[4] + sTransform[5] * gi[7];
            let inertia02 = sTransform[3] * gi[2] + sTransform[4] * gi[5] + sTransform[5] * gi[8];
            let inertia10 = sTransform[6] * gi[0] + sTransform[7] * gi[3] + sTransform[8] * gi[6];
            let inertia11 = sTransform[6] * gi[1] + sTransform[7] * gi[4] + sTransform[8] * gi[7];
            let inertia12 = sTransform[6] * gi[2] + sTransform[7] * gi[5] + sTransform[8] * gi[8];
            let inertia20 = sTransform[9] * gi[0] + sTransform[10] * gi[3] + sTransform[11] * gi[6];
            let inertia21 = sTransform[9] * gi[1] + sTransform[10] * gi[4] + sTransform[11] * gi[7];
            let inertia22 = sTransform[9] * gi[2] + sTransform[10] * gi[5] + sTransform[11] * gi[8];
            let __tmp__001 = inertia00 * sTransform[3] + inertia01 * sTransform[4] + inertia02 * sTransform[5];
            let __tmp__011 = inertia00 * sTransform[6] + inertia01 * sTransform[7] + inertia02 * sTransform[8];
            let __tmp__021 = inertia00 * sTransform[9] + inertia01 * sTransform[10] + inertia02 * sTransform[11];
            let __tmp__101 = inertia10 * sTransform[3] + inertia11 * sTransform[4] + inertia12 * sTransform[5];
            let __tmp__111 = inertia10 * sTransform[6] + inertia11 * sTransform[7] + inertia12 * sTransform[8];
            let __tmp__121 = inertia10 * sTransform[9] + inertia11 * sTransform[10] + inertia12 * sTransform[11];
            let __tmp__201 = inertia20 * sTransform[3] + inertia21 * sTransform[4] + inertia22 * sTransform[5];
            let __tmp__211 = inertia20 * sTransform[6] + inertia21 * sTransform[7] + inertia22 * sTransform[8];
            let __tmp__221 = inertia20 * sTransform[9] + inertia21 * sTransform[10] + inertia22 * sTransform[11];
            inertia00 = __tmp__001;inertia01 = __tmp__011;inertia02 = __tmp__021;
            inertia10 = __tmp__101;inertia11 = __tmp__111;inertia12 = __tmp__121;
            inertia20 = __tmp__201;inertia21 = __tmp__211;inertia22 = __tmp__221;
            inertia00 *= mass;inertia01 *= mass;inertia02 *= mass;
            inertia10 *= mass;inertia11 *= mass;inertia12 *= mass;
            inertia20 *= mass;inertia21 *= mass;inertia22 *= mass;
            let xx = sTransform[0] * sTransform[0];
            let yy = sTransform[1] * sTransform[1];
            let zz = sTransform[2] * sTransform[2];
            let xy = -sTransform[0] * sTransform[1];
            let yz = -sTransform[1] * sTransform[2];
            let zx = -sTransform[2] * sTransform[0];
            let cogInertia00 = yy + zz,cogInertia01 = xy,cogInertia02 = zx;
            let cogInertia10 = xy,cogInertia11 = xx + zz,cogInertia12 = yz;
            let cogInertia20 = zx,cogInertia21 = yz,cogInertia22 = xx + yy;
            inertia00 += cogInertia00 * mass;inertia01 += cogInertia01 * mass;inertia02 += cogInertia02 * mass;
            inertia10 += cogInertia10 * mass;inertia11 += cogInertia11 * mass;inertia12 += cogInertia12 * mass;
            inertia20 += cogInertia20 * mass;inertia21 += cogInertia21 * mass;inertia22 += cogInertia22 * mass;
            totalMass += mass;
            totalInertia00 += inertia00;totalInertia01 += inertia01;totalInertia02 += inertia02;
            totalInertia10 += inertia10;totalInertia11 += inertia11;totalInertia12 += inertia12;
            totalInertia20 += inertia20;totalInertia21 += inertia21;totalInertia22 += inertia22;
            s = n;
        }
        this._mass = totalMass;
        let localInertia=this.localInertia;
        localInertia[0] = totalInertia00;localInertia[1] = totalInertia01;localInertia[2] = totalInertia02;
        localInertia[3] = totalInertia10;localInertia[4] = totalInertia11;localInertia[5] = totalInertia12;
        localInertia[6] = totalInertia20;localInertia[7] = totalInertia21;localInertia[8] = totalInertia22;
        this._updateInvInertia();
        this._sleeping = false;
        this._sleepTime = 0;
    }
    getRotationTo(rotation) {
        let t=this._transform.elements;
        Method.setM3X3(rotation,t[3],t[4],t[5],t[6],t[7],t[8],t[9],t[10],t[11]);
    }
    setRotation(_rotation) {
        let rotation=Method.convertMat3(_rotation,this._tM0);
        let transform=this._transform.elements;
        Method.setTransformRotation(this._transform,rotation);
        this._transformInvInertia(transform);
        Method.copyTransform(this._transform,this._ptransform);
        this._updateShapeList();
        this._sleeping = false;
        this._sleepTime = 0;
    }
    rotate(_rotation) {
        let rotation=Method.convertMat3(_rotation,this._tM0);
        Method.rotateTransform(this._transform,rotation);
        this._transformInvInertia(this._transform.elements);
        Method.copyTransform(this._transform,this._ptransform);
        this._updateShapeList();
        this._sleeping = false;
        this._sleepTime = 0;
    }
    getOrientation() {
        Method.extractQuatFromTransform(this._transform,this._tQ0);
        return Method.copyQuat(this._tQ0);
    }
    getOrientationTo(orientation) {
        let tq=this._tQ0.elements;
        Method.extractQuatFromTransform(this._transform,tq);
        Method.setXYZW(orientation,tq[0],tq[1],tq[2],tq[3]);
    }
    setOrientation(_quaternion) {
        let quaternion=Method.convertQuat(_quaternion,this._tQ0);
        let transform=this._transform.elements;
        Method.setTransformOrientation(transform,quaternion);
        this._transformInvInertia(transform);
        Method.copyTransform(this._transform,this._ptransform);
        this._updateShapeList();
        this._sleeping = false;
        this._sleepTime = 0;
    }
    getTransform() {
        return Method.copyTransform(this._transform);
    }
    getTransformTo(transform) {
        Method.copyTransform(this._transform,transform);
    }
    setTransform(transform) {
        Method.copyTransform(transform,this._transform);
        this._transformInvInertia(this._transform.elements);
        Method.copyTransform(this._transform,this._ptransform);
        this._updateShapeList();
        this._sleeping = false;
        this._sleepTime = 0;
    }
    getLocalInertia() {
        return Method.copyMat3(this.localInertia);
    }
    getLocalInertiaTo(inertia) {
        let li=this.localInertia;
        Method.setM3X3(inertia,li[0],li[1],li[2],li[3],li[4],li[5],li[6],li[7],li[8]);
    }
    getMassData() {
        let md = new MassData();
        md.mass = this._mass;
        Method.copyMat3(this.localInertia,md.localInertia);
        return md;
    }
    // getMassDataTo(massData) {
    //     massData.mass = this._mass;
    //     Method.copyMat3(this.localInertia,massData.localInertia);
    // }
    // setMassData(massData) {
    //     this._mass = massData.mass;
    //     Method.copyMat3(massData.localInertia,this.localInertia);
    //     this._updateInvInertia();
    //     this._sleeping = false;
    //     this._sleepTime = 0;
    // }
    getRotationFactor() {
        return Method.copyVec3(this._rotFactor);
    }
    getRotationFactorTo(rotationFactor) {
        let rf=this._rotFactor.elements;
        Method.setXYZ(rotationFactor,rf[0],rf[1],rf[2]);
    }
    setRotationFactor(rotationFactor) {
        Method.convertVec3(rotationFactor,this._rotFactor);
        this._transformInvInertia(this._transform.elements);
        this._sleeping = false;
        this._sleepTime = 0;
    }
    getLinearVelocity() {
        return Method.copyVec3(this.vel);
    }
    getLinearVelocityTo(linearVelocity) {
        let v=this.vel;
        Method.setXYZ(linearVelocity,v[0],v[1],v[2]);
    }
    setLinearVelocity(linearVelocity) {
        if(this._type === CONSTANT.RIGID_BODY_TYPE_STATIC) {
            this.vel.fill(0);
        } else {
            let tv=Method.convertVec3(linearVelocity,this._tV0).elements;
            Method.copyElements(tv,this.vel);
        }
        this._sleeping = false;
        this._sleepTime = 0;
    }
    getAngularVelocity() {
        return Method.copyVec3(this.angVel);
    }
    getAngularVelocityTo(angularVelocity) {
        let v=this.angVel;
        Method.setXYZ(angularVelocity,v[0],v[1],v[2]);
    }
    setAngularVelocity(angularVelocity) {
        if(this._type === CONSTANT.RIGID_BODY_TYPE_STATIC) {
            this.angVel.fill(0);
        } else {
            let v=Method.convertVec3(angularVelocity,this._tV0).elements;
            Method.copyElements(v,this.angVel);
        }
        this._sleeping = false;
        this._sleepTime = 0;
    }
    addLinearVelocity(linearVelocityChange) {
        if(this._type !== CONSTANT.RIGID_BODY_TYPE_STATIC) {
            let v=Method.convertVec3(linearVelocityChange,this._tV0).elements;
            let t=this.vel;
            t[0]+=v[0];t[1]+=v[1];t[2]+=v[2];
        }
        this._sleeping = false;
        this._sleepTime = 0;
    }
    addAngularVelocity(angularVelocityChange) {
        if(this._type !== CONSTANT.RIGID_BODY_TYPE_STATIC) {
            let v=Method.convertVec3(angularVelocityChange,this._tV0).elements;
            let t=this.angVel;
            t[0]+=v[0];t[1]+=v[1];t[2]+=v[2];
        }
        this._sleeping = false;
        this._sleepTime = 0;
    }
    applyImpulse(_impulse,_positionInWorld) {
        let vel=this.vel,invMass=this._invMass,transform=this._transform.elements,angVel=this.angVel,invInertia=this.invInertia;
        let impulse=Method.convertVec3(_impulse,this._tV0).elements;
        let pw=Method.convertVec3(_positionInWorld,this._tV1).elements;
        vel[0] += impulse[0] * invMass;vel[1] += impulse[1] * invMass;vel[2] += impulse[2] * invMass;
        pw[0] -= transform[0];pw[1] -= transform[1];pw[2] -= transform[2];
        let v=this._tV2.elements;
        v[0]= pw[1] * impulse[2] - pw[2] * impulse[1];v[1] = pw[2] * impulse[0] - pw[0] * impulse[2];v[2] = pw[0] * impulse[1] - pw[1] * impulse[0];
        Method.rotateVec3(v,invInertia);
        angVel[0] += v[0];angVel[1] += v[1];angVel[2] += v[2];
        this._sleeping = false;
        this._sleepTime = 0;
    }
    applyLinearImpulse(_impulse) {
        let impulse=Method.convertVec3(_impulse,this._tV0).elements;
        let impX = impulse[0],impY = impulse[1],impZ = impulse[2];
        let vel=this.vel,invMass=this._invMass;
        vel[0] += impX * invMass;vel[1] += impY * invMass;vel[2] += impZ * invMass;
        this._sleeping = false;
        this._sleepTime = 0;
    }
    applyAngularImpulse(_impulse) {
        let impulse=Method.convertVec3(_impulse,this._tV0).elements;
        Method.rotateVec3(impulse,this.invInertia);
        let v=this.angVel;
        v[0] += impulse[0];v[1] += impulse[1];v[2] += impulse[2];
        this._sleeping = false;
        this._sleepTime = 0;
    }
    applyForce(_force,_positionInWorld) {
        let force=Method.convertVec3(_force,this._tV0).elements, torque=this.torque;
        let pw=Method.convertVec3(_positionInWorld,this._tV1).elements;
        let transform=this._transform.elements;
        let f=this.force;
        f[0] += force[0];f[1] += force[1];f[2] += force[2];
        pw[0] -= transform[0];pw[1] -= transform[1];pw[2] -= transform[2];
        let v=this._tV2.elements;
        v[0] = pw[1] * force[2] - pw[2] * force[1];v[1] = pw[2] * force[0] - pw[0] * force[2]; v[2] = pw[0] * force[1] - pw[1] * force[0];
        torque[0] += v[0];torque[1] += v[1];torque[2] += v[2];
        this._sleeping = false;
        this._sleepTime = 0;
    }
    applyForceToCenter(_force) {
        let force=Method.convertVec3(_force,this._tV0).elements;
        let f=this.force;
        f[0] += force[0];f[1] += force[1];f[2] += force[2];
        this._sleeping = false;
        this._sleepTime = 0;
    }
    applyTorque(_torque) {
        let torque=Method.convertVec3(_torque,this._tV0).elements;
        let t=this.torque;
        t[0] += torque[0];t[1] += torque[1];t[2] += torque[2];
        this._sleeping = false;
        this._sleepTime = 0;
    }
    getLinearContactImpulse() {
        return Method.copyVec3(this.linearContactImpulse);
    }
    getLinearContactImpulseTo(linearContactImpulse) {
        let lci=this.linearContactImpulse;
        Method.setXYZ(linearContactImpulse,lci[0],lci[1],lci[2]);
    }
    getAngularContactImpulse() {
        return Method.copyVec3(this.angularContactImpulse);
    }
    getAngularContactImpulseTo(angularContactImpulse) {
        let aci=this.angularContactImpulse;
        Method.setXYZ(angularContactImpulse,aci[0],aci[1],aci[2]);
    }
    getLocalPoint(worldPoint) {
        let wp=Method.convertVec3(worldPoint,this._tV0).elements;
        return Method.inverseTransformVec3(this._transform,wp,0,new Vec3());
    }
    getLocalPointTo(worldPoint,localPoint) {
        let wp=Method.convertVec3(worldPoint,this._tV0).elements, es=this._tV1.elements;
        Method.inverseTransformVec3(this._transform,wp,0,es);
        Method.setXYZ(localPoint,es[0],es[1],es[2]);
    }
    getLocalVector(worldVector) {
        let wv=Method.convertVec3(worldVector,this._tV0).elements;
        return Method.inverseTransformVec3(this._transform,wv,1,new Vec3());
    }
    getLocalVectorTo(worldVector,localVector) {
        let wv=Method.convertVec3(worldVector,this._tV0).elements,es=this._tV1.elements;
        Method.inverseTransformVec3(this._transform,wv,1,es);
        Method.setXYZ(localVector,es[0],es[1],es[2]);
    }
    getWorldPoint(localPoint) {
        let lp=Method.convertVec3(localPoint,this._tV0).elements;
        return Method.transformVec3(this._transform,lp,0,new Vec3());
    }
    getWorldPointTo(localPoint,worldPoint) {
        let lp=Method.convertVec3(localPoint,this._tV0).elements,es=this._tV1.elements;
        Method.transformVec3(this._transform,lp,0,es);
        Method.setXYZ(worldPoint,es[0],es[1],es[2]);
    }
    getWorldVector(localVector) {
        let lv=Method.convertVec3(localVector,this._tV0).elements;
        return Method.transformVec3(this._transform,lv,1,new Vec3());
    }
    getWorldVectorTo(localVector,worldVector) {
        let lv=Method.convertVec3(localVector,this._tV0).elements,es=this._tV1.elements;
        Method.transformVec3(this._transform,lv,1,es);
        Method.setXYZ(worldVector,es[0],es[1],es[2]);
    }
    wakeUp() {
        this._sleeping = false;
        this._sleepTime = 0;
    }
    sleep() {
        this._sleeping = true;
        this._sleepTime = 0;
    }

    //.................................................private...................................................

    _updateShapeList(){
        let s = this._shapeList;
        while(s != null) {
            let n = s._next;
            Method.multiplyTransform(s._localTransform,this._ptransform,s._ptransform);
            Method.multiplyTransform(s._localTransform,this._transform,s._transform);
            this._computeShapeAabb(s);
            if(s._proxy != null) {
                let transform=s._transform.elements,ptransform=s._ptransform.elements;
                let dX = transform[0] - ptransform[0],dY = transform[1] - ptransform[1],dZ = transform[2] - ptransform[2];
                let v = s.displacement.elements;
                v[0] = dX;v[1] = dY;v[2] = dZ;
                s._rigidBody._world._broadPhase.moveProxy(s._proxy,s._aabb,s.displacement);
            }
            s = n;
        }
    }
    _computeShapeAabb(s){
        s._geom._computeAabb(s._aabb,s._ptransform);
        let aabb=s._aabb.elements;
        let minX = aabb[0],minY = aabb[1],minZ = aabb[2];
        let maxX = aabb[3],maxY = aabb[4],maxZ = aabb[5];
        s._geom._computeAabb(s._aabb,s._transform);
        aabb[0] = minX < aabb[0] ? minX : aabb[0];aabb[1] = minY < aabb[1] ? minY : aabb[1];aabb[2] = minZ < aabb[2] ? minZ : aabb[2];
        aabb[3] = maxX > aabb[3] ? maxX : aabb[3];aabb[4] = maxY > aabb[4] ? maxY : aabb[4];aabb[5] = maxZ > aabb[5] ? maxZ : aabb[5];
    }
    _transformInvInertia(transform){
        let invInertia=this.invInertia,invLocalInertia=this.invLocalInertia;
        invInertia[0] = transform[3] * invLocalInertia[0] + transform[4] * invLocalInertia[3] + transform[5] * invLocalInertia[6];
        invInertia[1] = transform[3] * invLocalInertia[1] + transform[4] * invLocalInertia[4] + transform[5] * invLocalInertia[7];
        invInertia[2] = transform[3] * invLocalInertia[2] + transform[4] * invLocalInertia[5] + transform[5] * invLocalInertia[8];
        invInertia[3] = transform[6] * invLocalInertia[0] + transform[7] * invLocalInertia[3] + transform[8] * invLocalInertia[6];
        invInertia[4] = transform[6] * invLocalInertia[1] + transform[7] * invLocalInertia[4] + transform[8] * invLocalInertia[7];
        invInertia[5] = transform[6] * invLocalInertia[2] + transform[7] * invLocalInertia[5] + transform[8] * invLocalInertia[8];
        invInertia[6] = transform[9] * invLocalInertia[0] + transform[10] * invLocalInertia[3] + transform[11] * invLocalInertia[6];
        invInertia[7] = transform[9] * invLocalInertia[1] + transform[10] * invLocalInertia[4] + transform[11] * invLocalInertia[7];
        invInertia[8] = transform[9] * invLocalInertia[2] + transform[10] * invLocalInertia[5] + transform[11] * invLocalInertia[8];
        let __tmp__001 = invInertia[0] * transform[3] + invInertia[1] * transform[4] + invInertia[2] * transform[5];
        let __tmp__011 = invInertia[0] * transform[6] + invInertia[1] * transform[7] + invInertia[2] * transform[8];
        let __tmp__021 = invInertia[0] * transform[9] + invInertia[1] * transform[10] + invInertia[2] * transform[11];
        let __tmp__101 = invInertia[3] * transform[3] + invInertia[4] * transform[4] + invInertia[5] * transform[5];
        let __tmp__111 = invInertia[3] * transform[6] + invInertia[4] * transform[7] + invInertia[5] * transform[8];
        let __tmp__121 = invInertia[3] * transform[9] + invInertia[4] * transform[10] + invInertia[5] * transform[11];
        let __tmp__201 = invInertia[6] * transform[3] + invInertia[7] * transform[4] + invInertia[8] * transform[5];
        let __tmp__211 = invInertia[6] * transform[6] + invInertia[7] * transform[7] + invInertia[8] * transform[8];
        let __tmp__221 = invInertia[6] * transform[9] + invInertia[7] * transform[10] + invInertia[8] * transform[11];
        invInertia[0] = __tmp__001;invInertia[1] = __tmp__011;invInertia[2] = __tmp__021;
        invInertia[3] = __tmp__101;invInertia[4] = __tmp__111;invInertia[5] = __tmp__121;
        invInertia[6] = __tmp__201;invInertia[7] = __tmp__211;invInertia[8] = __tmp__221;
        let rf=this._rotFactor.elements;
        invInertia[0] *= rf[0];invInertia[1] *= rf[0];invInertia[2] *= rf[0];
        invInertia[3] *= rf[1];invInertia[4] *= rf[1];invInertia[5] *= rf[1];
        invInertia[6] *= rf[2];invInertia[7] *= rf[2];invInertia[8] *= rf[2];
    }
    _integrate(dt) {
        switch(this._type) {
            case CONSTANT.RIGID_BODY_TYPE_STATIC:
                this.vel.fill(0);
                this.angVel.fill(0);
                this.pseudoVel.fill(0);
                this.angPseudoVel.fill(0);
                break;
            case CONSTANT.RIGID_BODY_TYPE_DYNAMIC:case CONSTANT.RIGID_BODY_TYPE_KINEMATIC:
                const TPS=CONSTANT.SETTING_MAX_TRANSLATION_PER_STEP;
                const RPS=CONSTANT.SETTING_MAX_ROTATION_PER_STEP;
                let vel=this.vel,angVel=this.angVel;
                let translationX = vel[0] * dt,translationY = vel[1] * dt,translationZ = vel[2] * dt;
                let rotationX = angVel[0] * dt,rotationY = angVel[1] * dt,rotationZ = angVel[2] * dt;
                let translationLengthSq = translationX * translationX + translationY * translationY + translationZ * translationZ;
                let rotationLengthSq = rotationX * rotationX + rotationY * rotationY + rotationZ * rotationZ;
                if(translationLengthSq === 0 && rotationLengthSq === 0) {
                    return;
                }
                if(translationLengthSq > TPS * TPS) {
                    let l = TPS / Math.sqrt(translationLengthSq);
                    vel[0] *= l;vel[1] *= l;vel[2] *= l;
                    translationX *= l;translationY *= l;translationZ *= l;
                }
                if(rotationLengthSq > RPS * RPS) {
                    let l = RPS / Math.sqrt(rotationLengthSq);
                    angVel[0] *= l;angVel[1] *= l;angVel[2] *= l;
                    rotationX *= l;rotationY *= l;rotationZ *= l;
                }
                let transform=this._transform.elements;
                transform[0] += translationX;transform[1] += translationY;transform[2] += translationZ;
                let theta = Math.sqrt(rotationX * rotationX + rotationY * rotationY + rotationZ * rotationZ);
                let halfTheta = theta * 0.5;
                let rotationToSinAxisFactor;
                let cosHalfTheta;
                if(halfTheta < 0.5) {
                    let ht2 = halfTheta * halfTheta;
                    rotationToSinAxisFactor = 0.5 * (1 - ht2 * 0.16666666666666666 + ht2 * ht2 * 0.0083333333333333332);
                    cosHalfTheta = 1 - ht2 * 0.5 + ht2 * ht2 * 0.041666666666666664;
                } else {
                    rotationToSinAxisFactor = Math.sin(halfTheta) / theta;
                    cosHalfTheta = Math.cos(halfTheta);
                }
                let sinAxisX = rotationX * rotationToSinAxisFactor,sinAxisY = rotationY * rotationToSinAxisFactor,sinAxisZ = rotationZ * rotationToSinAxisFactor;
                let dqX = sinAxisX,dqY = sinAxisY,dqZ = sinAxisZ,dqW = cosHalfTheta;
                let tq=this._tQ0.elements;
                Method.extractQuatFromTransform(transform,tq);
                tq[0] = dqW * tq[0] + dqX * tq[3] + dqY * tq[2] - dqZ * tq[1];
                tq[1] = dqW * tq[1] - dqX * tq[2] + dqY * tq[3] + dqZ * tq[0];
                tq[2] = dqW * tq[2] + dqX * tq[1] - dqY * tq[0] + dqZ * tq[3];
                tq[3] = dqW * tq[3] - dqX * tq[0] - dqY * tq[1] - dqZ * tq[2];
                let l = tq[0] * tq[0] + tq[1] * tq[1] + tq[2] * tq[2] + tq[3] * tq[3];
                if(l > 1e-32) {
                    l = 1 / Math.sqrt(l);
                }
                tq[0] *= l;tq[1] *= l;tq[2] *= l;tq[3] *= l;
                Method.setTransformOrientation(transform,tq);
                this._transformInvInertia(transform);
                break;
        }
    }
    _integratePseudoVelocity() {
        let pseudoVel=this.pseudoVel,angPseudoVel=this.angPseudoVel;
        if(pseudoVel[0] * pseudoVel[0] + pseudoVel[1] * pseudoVel[1] + pseudoVel[2] * pseudoVel[2] === 0 &&
            angPseudoVel[0] * angPseudoVel[0] + angPseudoVel[1] * angPseudoVel[1] + angPseudoVel[2] * angPseudoVel[2] === 0) {
            return;
        }
        switch(this._type) {
            case CONSTANT.RIGID_BODY_TYPE_STATIC:
                pseudoVel.fill(0);
                angPseudoVel.fill(0);
                break;
            case CONSTANT.RIGID_BODY_TYPE_DYNAMIC:case CONSTANT.RIGID_BODY_TYPE_KINEMATIC:
                let translationX = pseudoVel[0],translationY = pseudoVel[1],translationZ = pseudoVel[2];
                let rotationX = angPseudoVel[0],rotationY = angPseudoVel[1],rotationZ = angPseudoVel[2];
                pseudoVel.fill(0);
                angPseudoVel.fill(0);
                let transform=this._transform.elements;
                transform[0] += translationX;transform[1] += translationY;transform[2] += translationZ;
                let theta = Math.sqrt(rotationX * rotationX + rotationY * rotationY + rotationZ * rotationZ);
                let halfTheta = theta * 0.5;
                let rotationToSinAxisFactor;
                let cosHalfTheta;
                if(halfTheta < 0.5) {
                    let ht2 = halfTheta * halfTheta;
                    rotationToSinAxisFactor = 0.5 * (1 - ht2 * 0.16666666666666666 + ht2 * ht2 * 0.0083333333333333332);
                    cosHalfTheta = 1 - ht2 * 0.5 + ht2 * ht2 * 0.041666666666666664;
                } else {
                    rotationToSinAxisFactor = Math.sin(halfTheta) / theta;
                    cosHalfTheta = Math.cos(halfTheta);
                }
                let sinAxisX = rotationX * rotationToSinAxisFactor, sinAxisY = rotationY * rotationToSinAxisFactor, sinAxisZ = rotationZ * rotationToSinAxisFactor;
                let dqX = sinAxisX,dqY = sinAxisY,dqZ = sinAxisZ,dqW = cosHalfTheta;
                let tq=this._tQ0.elements;
                Method.extractQuatFromTransform(transform,tq);
                tq[0] = dqW * tq[0] + dqX * tq[3] + dqY * tq[2] - dqZ * tq[1];
                tq[1] = dqW * tq[1] - dqX * tq[2] + dqY * tq[3] + dqZ * tq[0];
                tq[2] = dqW * tq[2] + dqX * tq[1] - dqY * tq[0] + dqZ * tq[3];
                tq[3] = dqW * tq[3] - dqX * tq[0] - dqY * tq[1] - dqZ * tq[2];
                let l = tq[0] * tq[0] + tq[1] * tq[1] + tq[2] * tq[2] + tq[3] * tq[3];
                if(l > 1e-32) {
                    l = 1 / Math.sqrt(l);
                }
                tq[0] *= l;tq[1] *= l;tq[2] *= l;tq[3] *= l;
                Method.setTransformOrientation(transform,tq);
                this._transformInvInertia(transform);
                break;
        }
    }
    _updateInvInertia(){
        let localInertia=this.localInertia,invLocalInertia=this.invLocalInertia,invLocalInertiaWithoutRotFactor=this.invLocalInertiaWithoutRotFactor,invInertia=this.invInertia;
        let transform=this._transform.elements,rf=this._rotFactor.elements;
        if(this._mass > 0 &&
            localInertia[0] * (localInertia[4] * localInertia[8] - localInertia[5] * localInertia[7]) -
            localInertia[1] * (localInertia[3] * localInertia[8] - localInertia[5] * localInertia[6]) +
            localInertia[2] * (localInertia[3] * localInertia[7] - localInertia[4] * localInertia[6]) > 0 &&
            this._type === CONSTANT.RIGID_BODY_TYPE_DYNAMIC) {
            this._invMass = 1 / this._mass;
            let d00 = localInertia[4] * localInertia[8] - localInertia[5] * localInertia[7];
            let d01 = localInertia[3] * localInertia[8] - localInertia[5] * localInertia[6];
            let d02 = localInertia[3] * localInertia[7] - localInertia[4] * localInertia[6];
            let d = localInertia[0] * d00 - localInertia[1] * d01 + localInertia[2] * d02;
            if(d < -1e-32 || d > 1e-32) {
                d = 1 / d;
            }
            invLocalInertia[0] = d00 * d;
            invLocalInertia[1] = -(localInertia[1] * localInertia[8] - localInertia[2] * localInertia[7]) * d;
            invLocalInertia[2] = (localInertia[1] * localInertia[5] - localInertia[2] * localInertia[4]) * d;
            invLocalInertia[3] = -d01 * d;
            invLocalInertia[4] = (localInertia[0] * localInertia[8] - localInertia[2] * localInertia[6]) * d;
            invLocalInertia[5] = -(localInertia[0] * localInertia[5] - localInertia[2] * localInertia[3]) * d;
            invLocalInertia[6] = d02 * d;
            invLocalInertia[7] = -(localInertia[0] * localInertia[7] - localInertia[1] * localInertia[6]) * d;
            invLocalInertia[8] = (localInertia[0] * localInertia[4] - localInertia[1] * localInertia[3]) * d;
            invLocalInertiaWithoutRotFactor[0] = invLocalInertia[0];
            invLocalInertiaWithoutRotFactor[1] = invLocalInertia[1];
            invLocalInertiaWithoutRotFactor[2] = invLocalInertia[2];
            invLocalInertiaWithoutRotFactor[3] = invLocalInertia[3];
            invLocalInertiaWithoutRotFactor[4] = invLocalInertia[4];
            invLocalInertiaWithoutRotFactor[5] = invLocalInertia[5];
            invLocalInertiaWithoutRotFactor[6] = invLocalInertia[6];
            invLocalInertiaWithoutRotFactor[7] = invLocalInertia[7];
            invLocalInertiaWithoutRotFactor[8] = invLocalInertia[8];
            invLocalInertia[0] = invLocalInertiaWithoutRotFactor[0] * rf[0];
            invLocalInertia[1] = invLocalInertiaWithoutRotFactor[1] * rf[0];
            invLocalInertia[2] = invLocalInertiaWithoutRotFactor[2] * rf[0];
            invLocalInertia[3] = invLocalInertiaWithoutRotFactor[3] * rf[1];
            invLocalInertia[4] = invLocalInertiaWithoutRotFactor[4] * rf[1];
            invLocalInertia[5] = invLocalInertiaWithoutRotFactor[5] * rf[1];
            invLocalInertia[6] = invLocalInertiaWithoutRotFactor[6] * rf[2];
            invLocalInertia[7] = invLocalInertiaWithoutRotFactor[7] * rf[2];
            invLocalInertia[8] = invLocalInertiaWithoutRotFactor[8] * rf[2];
        } else {
            this._invMass = 0;
            invLocalInertia.fill(0);
            invLocalInertiaWithoutRotFactor.fill(0);
            if(this._type === CONSTANT.RIGID_BODY_TYPE_DYNAMIC) {
                this._type = CONSTANT.RIGID_BODY_TYPE_STATIC;
            }
        }
        invInertia[0] = transform[3] * invLocalInertia[0] + transform[4] * invLocalInertia[3] + transform[5] * invLocalInertia[6];
        invInertia[1] = transform[3] * invLocalInertia[1] + transform[4] * invLocalInertia[4] + transform[5] * invLocalInertia[7];
        invInertia[2] = transform[3] * invLocalInertia[2] + transform[4] * invLocalInertia[5] + transform[5] * invLocalInertia[8];
        invInertia[3] = transform[6] * invLocalInertia[0] + transform[7] * invLocalInertia[3] + transform[8] * invLocalInertia[6];
        invInertia[4] = transform[6] * invLocalInertia[1] + transform[7] * invLocalInertia[4] + transform[8] * invLocalInertia[7];
        invInertia[5] = transform[6] * invLocalInertia[2] + transform[7] * invLocalInertia[5] + transform[8] * invLocalInertia[8];
        invInertia[6] = transform[9] * invLocalInertia[0] + transform[10] * invLocalInertia[3] + transform[11] * invLocalInertia[6];
        invInertia[7] = transform[9] * invLocalInertia[1] + transform[10] * invLocalInertia[4] + transform[11] * invLocalInertia[7];
        invInertia[8] = transform[9] * invLocalInertia[2] + transform[10] * invLocalInertia[5] + transform[11] * invLocalInertia[8];
        let __tmp__001 = invInertia[0] * transform[3] + invInertia[1] * transform[4] + invInertia[2] * transform[5];
        let __tmp__011 = invInertia[0] * transform[6] + invInertia[1] * transform[7] + invInertia[2] * transform[8];
        let __tmp__021 = invInertia[0] * transform[9] + invInertia[1] * transform[10] + invInertia[2] * transform[11];
        let __tmp__101 = invInertia[3] * transform[3] + invInertia[4] * transform[4] + invInertia[5] * transform[5];
        let __tmp__111 = invInertia[3] * transform[6] + invInertia[4] * transform[7] + invInertia[5] * transform[8];
        let __tmp__121 = invInertia[3] * transform[9] + invInertia[4] * transform[10] + invInertia[5] * transform[11];
        let __tmp__201 = invInertia[6] * transform[3] + invInertia[7] * transform[4] + invInertia[8] * transform[5];
        let __tmp__211 = invInertia[6] * transform[6] + invInertia[7] * transform[7] + invInertia[8] * transform[8];
        let __tmp__221 = invInertia[6] * transform[9] + invInertia[7] * transform[10] + invInertia[8] * transform[11];
        invInertia[0] = __tmp__001;invInertia[1] = __tmp__011;invInertia[2] = __tmp__021;
        invInertia[3] = __tmp__101;invInertia[4] = __tmp__111;invInertia[5] = __tmp__121;
        invInertia[6] = __tmp__201;invInertia[7] = __tmp__211;invInertia[8] = __tmp__221;
        invInertia[0] *= rf[0];invInertia[1] *= rf[0];invInertia[2] *= rf[0];
        invInertia[3] *= rf[1];invInertia[4] *= rf[1];invInertia[5] *= rf[1];
        invInertia[6] *= rf[2];invInertia[7] *= rf[2];invInertia[8] *= rf[2];
    }
}